function [state, P] = initialize_landmark(state, P, z, R)
% 初始化新路标
% 输入: state-状态向量, P-协方差, z-观测[r;φ], R-观测噪声
% 输出: state-扩展状态, P-扩展协方差

    x = state(1);
    y = state(2);
    theta = state(3);
    range = z(1);
    bearing = z(2);
    
    % 全局坐标
    mx = x + range * cos(theta + bearing);
    my = y + range * sin(theta + bearing);
    state = [state; mx; my];
    
    % 初始化雅可比
    n_old = length(P);
    Jx = [1, 0, -range * sin(theta + bearing);
          0, 1,  range * cos(theta + bearing)];
    Jz = [cos(theta + bearing), -range * sin(theta + bearing);
          sin(theta + bearing),  range * cos(theta + bearing)];
    
    % 完整的协方差扩展（包含新路标与所有已有状态的相关性）
    P_new = zeros(n_old + 2, n_old + 2);
    
    % 保留原有协方差
    P_new(1:n_old, 1:n_old) = P;
    
    % 新路标的协方差（来自机器人位姿不确定性和观测噪声）
    P_new(n_old+1:n_old+2, n_old+1:n_old+2) = Jx(:, 1:3) * P(1:3, 1:3) * Jx(:, 1:3)' + Jz * R * Jz';
    
    % 新路标与机器人位姿的相关性
    P_new(1:3, n_old+1:n_old+2) = P(1:3, 1:3) * Jx(:, 1:3)';
    P_new(n_old+1:n_old+2, 1:3) = P_new(1:3, n_old+1:n_old+2)';
    
    % 关键修复：新路标与已有路标的相关性（通过机器人位姿传播）
    if n_old > 3
        % 新路标与已有路标的协方差
        P_new(4:n_old, n_old+1:n_old+2) = P(4:n_old, 1:3) * Jx(:, 1:3)';
        P_new(n_old+1:n_old+2, 4:n_old) = P_new(4:n_old, n_old+1:n_old+2)';
    end
    
    % 保持对称性和数值稳定性
    P_new = (P_new + P_new') / 2;
    P_new = P_new + eye(n_old + 2) * 1e-9;
    P = P_new;
end

